
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode.h
  * @author     baiyang
  * @date       2021-8-12
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <assert.h>

#include "takeoff.h"
#include "auto_yaw.h"
#include <arming/gp_arming.h>
#include <motor/gp_motors_matrix.h>
#include <mc_attitude_control/mc_attitude_control.h>
#include <mc_position_control/mc_position_control.h>
#include <rc_channel/rc_channel.h>
#include <common/location/location.h>
#include <mc_loiter/mc_loiter.h>
#include <mc_wpnav/mc_wpnav.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
// Auto Pilot Modes enumeration
typedef enum {
    STABILIZE =     0,  // manual airframe angle with manual throttle
    ACRO =          1,  // manual body-frame angular rate with manual throttle
    ALT_HOLD =      2,  // manual airframe angle with automatic throttle
    AUTO =          3,  // fully automatic waypoint control using mission commands
    GUIDED =        4,  // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
    LOITER =        5,  // automatic horizontal acceleration with automatic throttle
    RTL =           6,  // automatic return to launching point
    CIRCLE =        7,  // automatic circular flight with automatic throttle
    LAND =          9,  // automatic landing with horizontal position control
    DRIFT =        11,  // semi-autonomous position, yaw and throttle control
    SPORT =        13,  // manual earth-frame angular rate control with manual throttle
    FLIP =         14,  // automatically flip the vehicle on the roll axis
    AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
    POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
    BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
    THROW =        18,  // throw to launch mode using inertial/GPS system, no pilot input
    AVOID_ADSB =   19,  // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
    GUIDED_NOGPS = 20,  // guided mode but only accepts attitude and altitude
    SMART_RTL =    21,  // SMART_RTL returns to home by retracing its steps
    FLOWHOLD  =    22,  // FLOWHOLD holds position with optical flow without rangefinder
    FOLLOW    =    23,  // follow attempts to follow another vehicle or ground station
    ZIGZAG    =    24,  // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
    SYSTEMID  =    25,  // System ID mode produces automated system identification signals in the controllers
    AUTOROTATE =   26,  // Autonomous autorotation
    AUTO_RTL =     27,  // Auto RTL, this is not a true mode, AUTO will report as this mode if entered to perform a DO_LAND_START Landing sequence
    TURTLE =       28,  // Flip over after crash
} ModeNumber;

// Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
typedef enum {
    AltHold_MotorStopped,
    AltHold_Takeoff,
    AltHold_Landed_Ground_Idle,
    AltHold_Landed_Pre_Takeoff,
    AltHold_Flying
} AltHoldModeState;

// RTL states
enum RtlSubMode {
    RTL_STARTING,
    RTL_INITIAL_CLIMB,
    RTL_RETURN_HOME,
    RTL_LOITER_AT_HOME,
    RTL_FINAL_DESCENT,
    RTL_LAND
};

// enum for RTL_ALT_TYPE parameter
enum RTLAltType {
    RTL_ALTTYPE_RELATIVE = 0,
    RTL_ALTTYPE_TERRAIN = 1
};

// return target alt type
enum ReturnTargetAltType {
    RTL_RELATIVE = 0,
    RTL_RANGEFINDER = 1,
    RTL_TERRAINDATABASE = 2
};

// enum for RTL_OPTIONS parameter
enum RtlOptions {
    // First pair of bits are still available, pilot yaw was mapped to bit 2 for symmetry with auto
    RtlIgnorePilotYaw    = (1U << 2),
};

enum GuidedSubMode {
    GUIDED_TakeOff,
    GUIDED_WP,
    GUIDED_Pos,
    GUIDED_PosVelAccel,
    GUIDED_VelAccel,
    GUIDED_Accel,
    GUIDED_Angle,
};

// enum for GUID_OPTIONS parameter
enum GuidedOptions {
    GuidedAllowArmingFromTX   = (1U << 0),
    // this bit is still available, pilot yaw was mapped to bit 2 for symmetry with auto
    GuidedIgnorePilotYaw      = (1U << 2),
    GuidedSetAttitudeTarget_ThrustAsThrust = (1U << 3),
    GuidedDoNotStabilizePositionXY = (1U << 4),
    GuidedDoNotStabilizeVelocityXY = (1U << 5),
    GuidedWPNavUsedForPosControl = (1U << 6),
};

typedef struct ModeBase* mode_base_t;
typedef struct mode_ops* mode_ops_t;

struct ModeBase {
    mode_ops_t ops;

    // convenience references to avoid code churn in conversion:
    ahrs_view *ahrs;
    Position_ctrl *pos_control;
    Attitude_ctrl *attitude_control;
    mc_loiter *loiter_nav;
    mc_wpnav *wp_nav;
    Motors_HandleTypeDef *motors;
    gp_arming_t arming;

    RC_HandleTypeDef *channel_roll;
    RC_HandleTypeDef *channel_pitch;
    RC_HandleTypeDef *channel_throttle;
    RC_HandleTypeDef *channel_yaw;

    // altitude above-ekf-origin below which auto takeoff does not control horizontal position
    bool *auto_takeoff_no_nav_active;
    float *auto_takeoff_no_nav_alt_cm;

    UserTakeOff *takeoff;
    AutoYaw *auto_yaw;

    bool *auto_takeoff_complete;
};

/** @ 
  * @brief  
  */
struct mode_ops {
    // returns a unique number specific to this mode
    ModeNumber (*mode_number)(mode_base_t mode);

    // child classes should override these methods
    bool (*init)(mode_base_t mode, bool ignore_checks);

    void (*exit)(mode_base_t mode);
    void (*run)(mode_base_t mode);
    bool (*requires_GPS)(mode_base_t mode);
    bool (*has_manual_throttle)(mode_base_t mode);
    bool (*allows_arming)(mode_base_t mode, ArmingMethod method);
    bool (*is_autopilot)(mode_base_t mode);
    bool (*has_user_takeoff)(mode_base_t mode, bool must_navigate);
    bool (*in_guided_mode)(mode_base_t mode);
    bool (*logs_attitude)(mode_base_t mode);
    bool (*allows_save_trim)(mode_base_t mode);
    bool (*allows_autotune)(mode_base_t mode);
    bool (*allows_flip)(mode_base_t mode);

    // return a string for this flightmode
    const char *(*name)(mode_base_t mode);
    const char *(*name4)(mode_base_t mode);
    bool (*is_taking_off)(mode_base_t mode);

    bool (*is_landing)(mode_base_t mode);

    // mode requires terrain to be present to be functional
    bool (*requires_terrain_failsafe)(mode_base_t mode);

    // functions for reporting to GCS
    bool (*get_wp)(mode_base_t mode, Location *loc);
    int32_t (*wp_bearing)(mode_base_t mode);
    uint32_t (*wp_distance)(mode_base_t mode);
    float (*crosstrack_error)(mode_base_t mode);

    // send output to the motors, can be overridden by subclasses
    void (*output_to_motors)(mode_base_t mode);

    // returns true if pilot's yaw input should be used to adjust vehicle's heading
    bool (*use_pilot_yaw)(mode_base_t mode);

    // return expected input throttle setting to hover:
    float (*throttle_hover)(mode_base_t mode);

    bool (*do_user_takeoff_start)(mode_base_t mode, float takeoff_alt_cm);

    // pause and resume a mode
    bool (*pause)(mode_base_t mode);
    bool (*resume)(mode_base_t mode);
};

typedef struct {
    struct ModeBase mode;
} ModeStabilize;

typedef struct {
    struct ModeBase mode;
} ModeAltHold;

typedef struct {
    struct ModeBase mode;
} ModeLoiter;

typedef struct {
    struct ModeBase mode;

    bool control_position; // true if we are using an external reference to control position

    uint32_t land_start_time;
    bool land_pause;
} ModeLand;

typedef struct {
    struct ModeBase mode;

    enum RtlSubMode _state;// records state of rtl (initial climb, returning home, etc)
    bool _state_complete; // set to true if the current state is completed

    struct {
        // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain
        Location origin_point;
        Location climb_target;
        Location return_target;
        Location descent_target;
        Vector3f_t destination_neu;
        bool land;
    } rtl_path;

    // Loiter timer - Records how long we have been in loiter
    uint32_t _loiter_start_time;

    bool terrain_following_allowed;
} ModeRTL;

typedef struct {
    struct ModeBase mode;

    // controls which controller is run (pos or vel):
    enum GuidedSubMode guided_mode;

    bool send_notification;     // used to send one time notification to ground station
    bool takeoff_complete;      // true once takeoff has completed (used to trigger retracting of landing gear)

    // guided mode is paused or not
    bool _paused;
} ModeGuided;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mode_ctor(mode_base_t mode, mode_ops_t ops);

// returns a unique number specific to this mode
static inline ModeNumber mode_number(mode_base_t mode) {
    assert(mode->ops->mode_number != NULL);
    return mode->ops->mode_number(mode);
}

// child classes should override these methods
static inline bool mode_init(mode_base_t mode, bool ignore_checks) {
    if (mode->ops->init != NULL) {
        return mode->ops->init(mode, ignore_checks);
    }

    return true;
}

static inline void mode_exit(mode_base_t mode) {
    if (mode->ops->exit != NULL) {
        mode->ops->exit(mode);
    }
}

static inline void mode_run(mode_base_t mode) {
    assert(mode->ops->run != NULL);
    mode->ops->run(mode);
}

static inline bool mode_requires_GPS(mode_base_t mode) {
    assert(mode->ops->requires_GPS != NULL);
    return mode->ops->requires_GPS(mode);
}

static inline bool mode_has_manual_throttle(mode_base_t mode) {
    assert(mode->ops->has_manual_throttle != NULL);
    return mode->ops->has_manual_throttle(mode);
}

static inline bool mode_allows_arming(mode_base_t mode, ArmingMethod method) {
    assert(mode->ops->allows_arming != NULL);
    return mode->ops->allows_arming(mode, method);
}

static inline bool mode_is_autopilot(mode_base_t mode) {
    if (mode->ops->is_autopilot != NULL) {
        return mode->ops->is_autopilot(mode);
    }

    return false;
}

static inline bool mode_has_user_takeoff(mode_base_t mode, bool must_navigate) {
    if (mode->ops->has_user_takeoff != NULL) {
        return mode->ops->has_user_takeoff(mode, must_navigate);
    }

    return false;
}

static inline bool mode_in_guided_mode(mode_base_t mode) {
    if (mode->ops->in_guided_mode != NULL) {
        return mode->ops->in_guided_mode(mode);
    }

    return false;
}

static inline bool mode_logs_attitude(mode_base_t mode) {
    if (mode->ops->logs_attitude != NULL) {
        return mode->ops->logs_attitude(mode);
    }

    return false;
}

static inline bool mode_allows_save_trim(mode_base_t mode) {
    if (mode->ops->allows_save_trim != NULL) {
        return mode->ops->allows_save_trim(mode);
    }

    return false;
}

static inline bool mode_allows_autotune(mode_base_t mode) {
    if (mode->ops->allows_autotune != NULL) {
        return mode->ops->allows_autotune(mode);
    }

    return false;
}

static inline bool mode_allows_flip(mode_base_t mode) {
    if (mode->ops->allows_flip != NULL) {
        return mode->ops->allows_flip(mode);
    }

    return false;
}

// return a string for this flightmode
static inline const char *mode_name(mode_base_t mode) {
    assert(mode->ops->name != NULL);
    return mode->ops->name(mode);
}

static inline const char *mode_name4(mode_base_t mode) {
    assert(mode->ops->name4 != NULL);
    return mode->ops->name4(mode);
}

static inline bool mode_is_taking_off(mode_base_t mode) {
    if (mode->ops->is_taking_off != NULL) {
        return mode->ops->is_taking_off(mode);
    }

    return false;
}

static inline void mode_takeoff_stop(mode_base_t mode) {
    takeoff_stop(mode->takeoff);
}

static inline bool mode_is_landing(mode_base_t mode) {
    if (mode->ops->is_landing != NULL) {
        return mode->ops->is_landing(mode);
    }

    return false;
}

// mode requires terrain to be present to be functional
static inline bool mode_requires_terrain_failsafe(mode_base_t mode) {
    if (mode->ops->requires_terrain_failsafe != NULL) {
        return mode->ops->requires_terrain_failsafe(mode);
    }

    return false;
}

// functions for reporting to GCS
static inline bool mode_get_wp(mode_base_t mode, Location *loc) {
    if (mode->ops->get_wp != NULL) {
        return mode->ops->get_wp(mode, loc);
    }

    return false;
};

static inline int32_t mode_wp_bearing(mode_base_t mode) {
    if (mode->ops->wp_bearing != NULL) {
        return mode->ops->wp_bearing(mode);
    }
    
    return 0;
}

static inline uint32_t mode_wp_distance(mode_base_t mode) {
    if (mode->ops->wp_distance != NULL) {
        return mode->ops->wp_distance(mode);
    }

    return 0;
}

static inline float mode_crosstrack_error(mode_base_t mode) {
    if (mode->ops->crosstrack_error != NULL) {
        return mode->ops->crosstrack_error(mode);
    }

    return 0.0f;
}

// return expected input throttle setting to hover:
static inline void mode_output_to_motors(mode_base_t mode) {
    if (mode->ops->output_to_motors != NULL) {
        mode->ops->output_to_motors(mode);
    } else {
        MotorsMat_output((MotorsMat_HandleTypeDef *)mode->motors);
    }
}

// return expected input throttle setting to hover:
static inline bool mode_use_pilot_yaw(mode_base_t mode) {
    if (mode->ops->use_pilot_yaw != NULL) {
        return mode->ops->use_pilot_yaw(mode);
    }

    return true;
}

// return expected input throttle setting to hover:
static inline float mode_throttle_hover(mode_base_t mode) {
    if (mode->ops->throttle_hover != NULL) {
        return mode->ops->throttle_hover(mode);
    }

    return ((MotorsMC_HandleTypeDef *)mode->motors)->_throttle_hover;
}

static inline bool mode_do_user_takeoff_start(mode_base_t mode, float takeoff_alt_cm) {
    if (mode->ops->do_user_takeoff_start != NULL) {
        return mode->ops->do_user_takeoff_start(mode, takeoff_alt_cm);
    }

    takeoff_start(mode->takeoff, takeoff_alt_cm);
    return true;
}

// pause and resume a mode
static inline bool mode_pause(mode_base_t mode) {
    if (mode->ops->pause != NULL) {
        return mode->ops->pause(mode);
    }

    return false;
}

static inline bool mode_resume(mode_base_t mode) {
    if (mode->ops->resume != NULL) {
        return mode->ops->resume(mode);
    }

    return false;
}

static inline Vector3f_t mode_get_vel_desired_cms(mode_base_t mode) {
    // note that position control isn't used in every mode, so
    // this may return bogus data:
    return posctrl_get_vel_desired_cms(mode->pos_control);
}

// helper functions
bool mode_is_disarmed_or_landed();
void mode_zero_throttle_and_relax_ac(bool spool_up); // spool_up,default false
void mode_zero_throttle_and_hold_attitude();
void mode_make_safe_spool_down(mode_base_t mode);
void mode_make_safe_ground_handling(mode_base_t mode, bool force_throttle_unlimited); //default, force_throttle_unlimited = false

// functions to control landing
// in modes that support landing
void mode_land_run_horizontal_control(mode_base_t mode);
void mode_land_run_vertical_control(mode_base_t mode, bool pause_descent); // pause_descent,default false
int32_t mode_get_alt_above_ground_cm(mode_base_t mode);
void mode_land_run_normal_or_precland(mode_base_t mode, bool pause_descent);

// default pause_descent = false
static inline void mode_land_run_horiz_and_vert_control(mode_base_t mode, bool pause_descent) {
    mode_land_run_horizontal_control(mode);
    mode_land_run_vertical_control(mode, pause_descent);
}

AltHoldModeState mode_get_alt_hold_state(float target_climb_rate_cms);

// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
void mode_update_simple_mode(mode_base_t mode);
bool mode_set_mode(mode_base_t mode, ModeNumber mode_number, ModeReason reason);
void mode_set_land_complete(mode_base_t mode, bool b);
void mode_set_throttle_takeoff();
uint16_t mode_get_pilot_speed_dn(mode_base_t mode);

// method shared by both Guided and Auto for takeoff.  This is
// waypoint navigation but the user can control the yaw.
void mode_auto_takeoff_run(mode_base_t mode);
void mode_auto_takeoff_set_start_alt(mode_base_t mode);
void mode_auto_takeoff_start(mode_base_t mode, float complete_alt_cm, bool terrain_alt);

// initiate user takeoff - called when MAVLink TAKEOFF command is received
bool mode_do_user_takeoff(mode_base_t mode, float takeoff_alt_cm, bool must_navigate);


/// ModeStabilize
void mode_stabilize_ctor(ModeStabilize* mode_stabilize);

/// ModeAltHold
void mode_althold_ctor(ModeAltHold* mode_althold);

/// ModeLoiter
void mode_loiter_ctor(ModeLoiter* mode_loiter);

/// ModeLand
void mode_land_ctor(ModeLand* mode_land);
void mland_do_not_use_GPS(mode_base_t mode);
static inline bool mland_controlling_position(ModeLand* mode_land) { return mode_land->control_position; }
static inline void mland_set_land_pause(ModeLand* mode_land, bool new_value) { mode_land->land_pause = new_value; }

/// ModeRTL
void mode_rtl_ctor(ModeRTL* mode_rtl);
void mrtl_run2(mode_base_t mode, bool disarm_on_land);
void mrtl_descent_start(mode_base_t mode);
void mrtl_descent_run(mode_base_t mode);
void mrtl_land_start(mode_base_t mode);
void mrtl_land_run(mode_base_t mode, bool disarm_on_land);
enum RTLAltType mrtl_get_alt_type();
void mrtl_restart_without_terrain(mode_base_t mode);
bool mrtl_reached_wp_destination(mode_base_t mode);
static inline void mrtl_set_descent_target_alt(mode_base_t mode, uint32_t alt) { ((ModeRTL*)mode)->rtl_path.descent_target.alt = alt; }
static inline bool mrtl_state_complete(mode_base_t mode) { return ((ModeRTL*)mode)->_state_complete; }

/// ModeGuided
void mode_guided_ctor(ModeGuided* mode_guided);
uint32_t mguided_get_timeout_ms();

// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
bool mguided_set_attitude_target_provides_thrust();

// returns true if GUIDED_OPTIONS param specifies position should be controlled (when velocity and/or acceleration control is active)
bool mguided_stabilizing_pos_xy();

// returns true if GUIDED_OPTIONS param specifies velocity should  be controlled (when acceleration control is active)
bool mguided_stabilizing_vel_xy();

// returns true if GUIDED_OPTIONS param specifies waypoint navigation should be used for position control (allow path planning to be used but updates must be slower)
bool mguided_use_wpnav_for_position_control();

// initialise guided mode's angle controller
void mguided_angle_control_start(mode_base_t mode);

// angle_control_run - runs the guided angle controller
// called from guided_run
void mguided_angle_control_run(mode_base_t mode);

// set_destination - sets guided mode's target destination
bool mguided_set_destination(mode_base_t mode, const Vector3f_t* destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool terrain_alt);

// sets guided mode's target from a Location object
bool mguided_set_destination2(mode_base_t mode, const Location* dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);

// set_velaccel - sets guided mode's target velocity and acceleration
void mguided_set_accel(mode_base_t mode, const Vector3f_t* acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request);

// set_velocity - sets guided mode's target velocity
void mguided_set_velocity(mode_base_t mode, const Vector3f_t* velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request);

// set_velaccel - sets guided mode's target velocity and acceleration
void mguided_set_velaccel(mode_base_t mode, const Vector3f_t* velocity, const Vector3f_t* acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request);

// set_destination_posvel - set guided mode position and velocity target
bool mguided_set_destination_posvel(mode_base_t mode, const Vector3f_t* destination, const Vector3f_t* velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);

// set_destination_posvelaccel - set guided mode position, velocity and acceleration target
bool mguided_set_destination_posvelaccel(mode_base_t mode, const Vector3f_t* destination, const Vector3f_t* velocity, const Vector3f_t* acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);

// Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
void mguided_set_angle(mode_base_t mode, const Quat_t *attitude_quat, const Vector3f_t *ang_vel, float climb_rate_cms_or_thrust, bool use_thrust);

// limit_clear - clear/turn off guided limits
void mguided_limit_clear();

// limit_set - set guided timeout and movement limits
void mguided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);

// limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
//  only called from AUTO mode's auto_nav_guided_start function
void mguided_limit_init_time_and_pos();

// limit_check - returns true if guided mode has breached a limit
//  used when guided is invoked from the NAV_GUIDED_ENABLE mission command
bool mguided_limit_check();

const Vector3p* mguided_get_target_pos();
const Vector3f_t* mguided_get_target_vel();
const Vector3f_t* mguided_get_target_accel();
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



